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(57) Abstract: The invention relates to a system for estimating the 
position, speed and orientation of a vehicle (10) comprising means 
for determining components of two non-collinear constant pointing 
vectors (formula I), according to the vehicle body axes, means for 
determining components of said non-collinear constant pointing vec- 
tors (formula II) according to the Earth's axes (A), means for deter- 
mining three components of the angular speed (formula III) of the 
vehicle in the axes of the body thereof, means for correcting said 
angular speed (formula III) with the u ro correction and obtaining a 
corrected angular speed (formula IV), a control module (14) using 
a control principle in order to calculate said u ro correction according 
to a formula [formula V], wherein a is a positive gradient. When 
said corrected speed (formula IV) is used as the input of an integra- 
tion module of cinematic equations, said equations are stable with 
respect to ISS and the estimation error of the directive cosines ma- 
trix (formula VI) and Euler (formula VII) angles remains bounded. 

(57) Resumen: Sistema para estimar position, velocidad y orienta- 
tion de un vehiculo (10), que comprende medios para determinar las 
componentes, segun ejes cuerpo del vehiculo, de dos vectores unita- 
rios constantes no colineales (formula I); medios para determinar las 
componentes, segun ejes tierra, de dichos vectores unitarios constan- 
tes no colineales (formula II); medios para determinar las tres compo- 
nentes de velocidad angular (formula III) del vehiculo en ejes cuerpo; 
medios para corregir dicha velocidad angular (formula HT) con una 
correccion w ro y obtener una velocidad angular corregida (formula 
IV); un modulo de control (14), que implementa una ley de con- 
trol para calcular dicha correccion u^ donde dicha ley de control es: 
(formula V) siendo a 



[Continua en la pdgina siguiente] 



WO 2005/083358 Al I Hill llllllll II llllll Mill IIMI lllll llll I II III lllll lllll Mill lllll lllll llll lllllll llll Mil llll 



(81) Estados designados (a menos que se indique otra cosa, 
para toda clase de proteccion nacional admisible): AE, 
AG, AL, AM, AT, AU, AZ, BA, BB, BG, BR, BW, BY, 
BZ, CA, CH, CN, CO, CR, CU, CZ, DE, DK, DM, DZ, 
EC, EE, EG, ES, FI, GB, GD, GE, GH, GM, HR, HU, ID, 
IL, IN, IS, JP, KE, KG, KP, KR, KZ, LC, LK, LR, LS, LT, 
LU, LV, MA, MD, MG, MK, MN, MW, MX, MZ, NA, NI, 
NO, NZ, OM, PG, PH, PL, PT, RO, RU, SC, SD, SE, SG, 
SK, SL, SM, SY, TJ, TM, TN, TR, TT, TZ, UA, UG, US, 
UZ, VC, VN, YU, ZA, ZM, ZW. 

(84) Estados designados (a menos que se indique otra cosa, 
para toda clase de proteccion regional admisible): ARIPO 



(BW, GH, GM, KE, LS, MW, MZ, NA, SD, SL, SZ, TZ, 
UG, ZM, ZW), euroasiatica (AM, AZ, BY, KG, KZ, MD, 
RU, TJ, TM), europea (AT, BE, BG, CH, CY, CZ, DE, DK, 
EE, ES, FI, FR, GB, GR, HU, IE, IS, IT, LT, LU, MC, NL, 
PL, PT, RO, SE, SI, SK, TR), OAPI (BF, BJ, CF, CG, CI, 
CM, GA, GN, GQ, GW, ML, MR, NE, SN, TD, TG). 

Publicada: 

— con informe de bus que da intemacional 

Para codigos de dos letras y otras abreviaturas, vease la seccion 
"Guidance Notes on Codes and Abbreviations" que aparece al 
principio de cada numero regular de la Gaceta del PCT. 



un escalar positivo, de forma que al utilizar dicha velocidad angular corregida (formula IV) como entrada a un modulo de integration 
de las ecuaciones cinematicas, estas son estables en sentido ISS y el error en la estimation de la matriz de cosenos directores (formula 
VI) y de los angulos de Euler (formula VII) se mantiene acotado. 
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SISTEMA Y METODO DE FUSION DE SENSORES PARA ESTIMAR POSICION, 
VELOCIDAD Y ORIENTACION DE UN VEHICULO, ESPEC1ALMENTE UNA 
AERONAVE 

Campo de la invencion 

5 El presente invento se refiere a un metodo y sistema para integrar datos 

procedentes de diversos sensores y estimar la posicion, velocidad y orientacion de un 
vehfculo. Es especialmente apropiada su aplicacion a Aeronaves No Tripuladas (UAV) 
que incorporan sensores de bajo coste para: 

- determinar la actitud y orientacion del vehfculo; 
10 - determinar su posicion y velocidad; 

- navegar durante periodos de tiempo limitados sin GPS. 

La invencion se integra dentro de los cainpos de fusion de sensores, 
determinacion de actitud y navegacion inercial. 

Antecedentes de la invencion 
15 Existen diversos metodos para estimar la posicion, velocidad y orientacion de 

un vehfculo: 

1.- Sistema de navegacion inercial o INS: Integra las aceleraciones y 
velocidades angulares proporcionadas por una Unidad de Medida Inercial (IMU) para 
calcular la posicion, velocidad y orientacion del vehfculo. Como esta integracion es 
20 neutralmente estable, los errores se acumulan y las soluciones obtenidas divergen 
rapidamente, salvo que se utilicen sensores de muy alta calidad, que elevan el coste y 
peso del sistema. 

La solucion pasa por estabilizar la integracion mediante realimentacion de 
bucle cerrado con medidas de otros sensores que no deriven con el tiempo. Con este 
25 proposito, se utilizan medidas de velocidad aerodinamica y las proporcionadas por 
GPS y magnetometros. Se emplean diversos metodos para integrar todas o parte de 
estas medidas: 

1.1. - Filtro de Kalman Lineal: Es el de mayor simplicidad y menor coste 
computacional y, por tanto, muy atractivo para aplicaciones de bajo coste. El 

30 inconveniente es que solo es aplicable a sistemas dinamicos lineales o linealizados. 
Por tanto, solamente en casos concretos podra utilizarse. 

1.2. - Filtro de Kalman Complementario: Se emplean los algoritmos INS para 
integrar las medidas de una IMU, que puede ser de bajo coste. Las entradas al INS se 
corrigen con las salidas de un filtro de Kalman lineal constituido por un modelo de error 

35 del INS y de las medidas y alimentado por el error entre la posicion y velocidad 
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estimadas por el INS y las medidas por el resto de sensores. El problema es que la 
linealizacion que conlleva el modelo de error implica que no se garantice convergencia 
global y actualizaciones espureas podrfan llevar a la divergencia del sistema; ademas, 
como todos los filtros de Kalman, su diseno implica el conocimiento de la estadistica, 
5 tanto del ruido de medida como el del proceso y que estos sean blancos, gausianos y 
no correlacionados, lo que en el caso de sensores de bajo coste se da con poca 
frecuencia. Su coste computacional oscila entre moderado y alto dependiendo de la 
dimension de los vectores de estado y medidas. 

1.3.- Filtro de Kalman Extendido: Probablemente es el mas utilizado al ser mas 

10 preciso que el filtro Kalman estandar. Puede estimar la dinamica del vehiculo que, en 
general, no es lineal, ya que admite terminos no lineales tanto en el modelo como en 
las medidas. Tiene un riesgo mayor de divergencia que el Kalman estandar, pues las 
ecuaciones de covarianza se basan en el sistema linealizado y no en el real no lineal. 
Se ha probado su estabilidad local asintotica, pero no se puede garantizar su 

15 estabilidad global. Ademas de compartir con el resto de los filtros Kalman la necesidad 
de conocer las estadfsticas de ruido y medida, su coste computacional es alto. 
2.- Metodo Estatico 6 "Single Frame": donde a diferencia de los metodos de 
filtrado anteriores, se realiza una estimacion estatica, consistente en obtener la 
orientacion a partir de un conjunto de medidas de vectores unitarios tanto en ejes 

20 cuerpo como en ejes referenda. Casi todos los algoritmos (Davenport's, QUEST, 
FOAM...) se basan en resolver el problema de Wahba, que consiste en encontrar una 
matriz ortogonal, con determinante +1, que minimice una funcion de coste compuesta 
por la suma ponderada de los cuadrados del error entre los vectores unitarios en ejes 
cuerpo y el resultado de transformar los vectores en ejes referenda por la matriz que 

25 se busca. Suele aplicarse en sistemas espaciales, donde se obtienen los vectores 
unitarios por apuntamiento al Sol y otras estrellas. 

Hay algunos ejemplos de aplicacion a una estimacion dinamica, pero es a base 
de su integracion en un filtro de Kalman, lineal o complementario, con las desventajas 
enunciadas anteriormente. 

30 Descripcion de la invencion 

La invencion se refiere a un sistema de acuerdo con la reivindicacion 1 y a un 
metodo segun la reivindicacion 7. Realizaciones preferidas del sistema y del metodo 
se definen en las reivindicaciones dependientes. 

Dadas las carencias de la tecnica anterior que pueden resumirse en: 

35 • falta de garantfa de convergencia global; 
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• necesidad de conocer la estadfstica de ruido y proceso; 

• necesidad de que los ruidos de medida y proceso sean blancos, gausianos y 
no correlacionados; 

• coste computacional de moderado a alto; 

el objetivo de la presente invencion consiste en suministrar un sistema de fusion de 
sensores de acuerdo con la reivindicacion 1 y un metodo segun la reivindicacion 7 
que: 

• es convergente globalmente en sentido ISS (Input to State Stability); 

• no requiere modelos estadfsticos complejos; y 

• presenta un coste computacional bajo. 

Para conseguir los objetivos anteriores hay que resolver el problema de 
estabilizar la integracion de las ecuaciones cinematicas de forma global y sin que 
computacionalmente sea costoso. Para lograrlo, en vez de utilizar metodos de 
estirnacion estadfstica como en el filtro de Kalman, la presente invencion plantea el 
problema como uno de control no lineal y lo resuelve haciendo uso de la teoria de 
estabilidad de Lyapunov, encontrando una ley de control que proporciona las 
correcciones que hay que aplicar a la velocidad angular para estabilizar la 

integracion de las ecuaciones cinematicas de forma global asintotica. 

El planteamiento del problema de control no lineal y su solucion ha sido 
desarrollado de la siguiente forma: 

a.- Planteamiento: Sean g b ,e b dos vectores unitarios constantes expresados en unos 
ejes fijos al vehfculo (ejes cuerpo), y g n e t los mismos vectores expresados en ejes 

locales (ejes tierra). 

Si B es la matriz de cosenos directores, se tiene: 

e b = Be t 

Integrando las ecuaciones cinematicas con la velocidad angular medida, cd b , 

obtendrfamos una matriz de cosenos directores B, que sabemos erronea por las 
imprecisiones de los sensores. Por tanto, aplicaremos una correccion u m a la 

velocidad angular medida. Llamando cb b = & b +u m a la velocidad angular corregida, e 

integrandola obtendremos la matriz de cosenos directores B . A partir de esta matriz 
de cosenos directores pueden obtenerse unos vectores estimados g b ,e b : 
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ft = B S, 
e b = Be t 

Se trata ahora de determinar la correcci6n u w para que se anule el error 

ft = ft~ft 

e b — e b e b 

Este es un problema de control no lineal que se va a resolver utilizando una 
extension del concepto de funcion de Lyapunov denominado Funcion de Control de 
Lyapunov (Clf) [refl] en los siguientes pasos: 

1 . Formamos el sistema 



z = f( t z 9 u 4B ) 9 z = 



L S bJ 



,/(0,0) = 0 [1.2] 



2. Encontraremos una Clf para el sistema anterior, es decir, una funcion continua 
derivable, definida positiva y no acotada radialmente J : □ 6 □ + tal que: 



infi^W/fe^)l<0 5 Vz*0 [1.3] 



15 3. Encontraremos una ley de control u 0) =a(z) tal que si W(z) es una funcion 

definida positiva, se cumpla: 

JL( z )f( zM z))<-W(z) [1.4] 

20 4. El Teorema de LaSalle-Yoshizawa [refl] nos garantiza que el sistema [1.2] con 

-a(z) es global, uniforme y asintoticamente estable (GUAS) para el punto 

de equilibrio z=0. 
b.- Resolucion: 

1 . Derivando [1.1] obtenemos el sistema: 

25 ft— *x&+«.*& [15] 

2. Probaremos la siguiente Clf: 

J(z) = ^[g b -g b +e b -e b ] [1.6] 



cuya derivada segCin la solucion de [1 .5] es: 
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9J r / . 

— (z)f(z 9 uj = 
az 

-g b -(& b xg b ) + g b - K x g b ) t1 7] 

Teniendo en cuenta las propiedades del producto vectorial queda: 

—(z)f(z,u b ) = -u m • (g b xg b +e b xe b ) [1 .8] 

3. Tomamos la siguiente ley de control: 

\ = ^(&xg, + ^x^) [1] 
donde a es un escalar positive 

Llamando T = g b x g b + e b x e b y substituyendo en [1 .8] se tiene: 
^.(z) /(z,^) = -cr(f . f ) = -cr|r| [1.10] 

4. Puesto que r 2 es una funcion definida positiva, se cumple [1.4] y por el 
teorema de LaSalle-Yoshizawa [refl] el sistema [1.5] con u^-oT es global, 

uniforme y asintoticamente estable (GUAS) para el punto de equilibrio z=0. 
c-- Robustez: La Ley de Control anterior cumple con los objetivos propuestos pero 
solo si tenemos medidas perfectas de los vectores g b9 e b , lo que en la practica la 

convertina en inutil. Sin embargo, como se demostrara seguidamente, no es asf y esta 
ley de control sigue siendo valida con medidas erroneas, siempre que esten acotadas, 
como ocurre en la practica. 

Supongamos que las medidas g b ,e b contienen errores aditivos. El vector 
verdadero sera el medido mas los errores de medida: 

g = g + g j 1/M j 
e = e + e 

Sustituyendo en la definicion de fse tiene: 

f =(g b +g b )><g b +(e b +e b )xe b 

= [(ft^&) + (%^)] 
+ [(&x&) + (^x4)] 
= f + f 



[1.12] 



Como Cinicamente conocemos el valor medido de f\ solo podremos utilizar 
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fen las leyes de control, y la ecuacion [1.10] se modifica para ser: 

f^(z)/(z,0 = -^((f-f)f) 

dz [1.13] 

-l2 



-a 



r +o-(r-r) 



con lo que ya no se puede garantizar que el sistema [1.5] con u^-dt sea global, 

uniforme y asintoticamente estable (GUAS) para el punto de equilibrio z=0. Sin 
5 embargo, demostraremos que es ISS (input to state stable) [ref2] [ref3] [refl], es decir, 
que se cumple la propiedad: 

\z{f)\ < j3(\z(0)\,t) + /(sup If (r)|) [1.14] 

donde J3 es una funcion de clase KL y y es una funcion de clase K[ref1]. 
Utilizando la inigualdad de Young se tiene: 

10 r f<f f+i(r r) [1.15] 

y sustituyendo en [1.13]: 

^(z)f(z 9 uJ*^J5a\lf+a\ff [1.16] 

que prueba que ^-(z) / (z 9 u m ) < 0 ? Vz ^ 0 , siempre que |r| 2 > — ||fj| 2 . 

oz 3 

15 Como J(z) y r 2 (z) son definidas positivas y no estan acotadas radialmente y 

J(z) es lisa, existen funciones y l ,y 2 ,7 % de clase K M tales que: 

ri {\z\)<j{z)<r 2 (\z\) 



Por tanto, 



[1.17] 

r 3 (H)<r 2 (z) 



^)|>^ 1 [|||f||lj=>i<0 [1.18] 



20 Lo que implica que si: 



entonces: 



que a su vez implica que 
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[1.19] 



Si, por otra parte 



Ho)|>rr'(f|r£) 



resulta que 



Ht)\<n l °r 2 {Ho)\) 



[1.20] 



Por tanto, teniendo en cuenta [1.19] y [1.20] se demuestra que z(i) esta 
acotada globalmente: 



Ademas, por el Teorema C.2 [refl] (formulado en [ref2]) el sistema [1.5] con la 

Ley de Control u m = df es ISS con respecto a los errores de los sensores f . 

Asf, la presente invencion se refiere a un sistema para estimar posicion, 
velocidad y orientacion de un vehfculo, como puede ser una aeronave no tripulada, 
que incluye: 

medios para determinar las componentes, segun ejes cuerpo del vehfculo, de 
dos vectores unitarios constantes no colineales g b ,e b ; 

medios para determinar las componentes, segun ejes tierra, de dichos vectores 
unitarios constantes no colineales g t9 e t ; y 

medios para determinar las tres componentes de la velocidad angular S) b del 

vehfculo en ejes cuerpo. 

El sistema de fusion de sensores de la invencion incluye ademas: 

medios para corregir la velocidad angular cb b con una correccion u w y obtener 

una velocidad angular corregida cb b = a> b + ; 

un modulo de integracion de las ecuaciones cinematicas del vehfculo, que 
recibe como entrada la velocidad angular corregida cb b y proporciona la matriz de 

transformacion de ejes tierra a ejes cuerpo del vehfculo B y la orientacion del vehfculo 
en forma de angulos de Euler 3> ; 




y converge al conjunto residual: 
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un modulo de sintesis de las componentes en ejes cuerpo de los dos vectores 
unitarios constantes no colineales para proporcionar una estimation de dichos 
vectores no colineales en ejes cuerpo g b ,e b , donde dicha estimation se calcula segun: 

S b = Bg t 
e b = Be t 

5 un modulo de control, que implementa una ley de control para calcular dicha 

correction , donde dicha ley de control es: 

u m = <r(g b Xg b +d t X'e t ) [1] 
siendo a un escalar positive, 

de forma que al aplicar esta correction u m a la velocidad angular medida a> b y utilizar 

10 la velocidad angular resultante d> b -fy+u^ como entrada al modulo de integration de 

las ecuaciones cinematicas, estas son estables en sentido ISS y el error en la 

estimation de la matriz de cosenos directores B y de los angulos de Euler <D se 
mantiene acotado. 

Preferiblemente dichos vectores unitarios no colineales g,e son gravedad local 
15 g y proyeccion del campo magnetico sobre el piano perpendicular a la gravedad e . 

Preferiblemente, los medios para determinar las componentes, segun ejes 
cuerpo del vehiculo, de los dos vectores unitarios constantes no colineales g b9 e b 
incluyen: 

una Unidad de Medida Inercial (IMU), que incluye un conjunto de al menos tres 
20 giroscopos y al menos tres acelerometros situados segtin los ejes cuerpo del vehiculo; 

un magnetometro con capacidad para medir el campo magnetico terrestre 
segun los ejes cuerpo del vehiculo; 

sensores de presion estatica y diferencial; 
dos veletas para medir los angulos de ataque y resbalamiento; 
25 - un modulo de adquisicion y proceso de la velocidad angular, configurado para 
adquirir la velocidad angular a> b (t) medida por los giroscopos y retrasarla para obtener 

& b (t-T)\ 

un modulo de adquisicion y proceso de datos configurado para adquirir la 
fuerza espeeffica a b (t) medida por los acelerometros, la presion estatica p s (t) , la 

30 presion diferencial p d (t), el angulo de ataque a{t) , el angulo de resbalamiento fi(t) 
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y el valor del campo magnetico terrestre m b (t) medido en el magnetometro, 
retrasarlos y procesarlos para calcular la velocidad verdadera respecto al aire v(t - r) , 
la velocidad aerodinamica en ejes cuerpo v b (t-r) segun: 



calcular la derivada numerica de la velocidad aerodinamica en ejes cuerpo V b (t-r) , 
calcular la gravedad local en ejes cuerpo g b segun: 



y calcular la proyeccion del campo magnetico terrestre sobre el piano horizontal 
perpendicular a la gravedad local e{t -r) segun: 



Los medios para conocer las componentes, segtin ejes tierra, de los dos 
vectores unitarios constantes no colineales g t ,e t pueden incluir: 

un receptor GPS (Global Positioning System), 
y en dicho modulo de adquisicion y proceso de datos (17) se adquieren los datos 
proporcionados por el GPS, se procesan y se utilizan para calcular las componentes, 
segun los ejes tierra, de los dos vectores unitarios constantes no colineales g n e t . 

El sistema puede preferiblemente incluir un filtro de Savitzky-Golay en el que se 
calcula v b , derivada numerica de v b . 

De acuerdo con un aspecto adicional de la presente invencion, se proporciona 
un sistema de fusion de sensores para estimar posicion y velocidad de un vehfculo, en 
particular una aeronave no tripulada, que preferiblemente ademas incluye: 

medios de adquisicion de datos de un grupo de sensores situados en el 
vehfculo, como podrfa ser un receptor GPS (Global Positioning System) y un sensor 
de presion estatica, que proporcionan posicion y velocidad en unos ejes fijos a tierra 



en caso de que los medios para conocer las componentes, segtin los ejes 
cuerpo del vehfculo, de los vectores unitarios constantes mencionados anteriormente 
no incluyesen medios de adquisicion de datos de un conjunto de tres acelerometros 
ituados segtin los ejes cuerpo del vehfculo, estos se incluiran para proporcionar la 



v cos a cos ft 
Csinacos p 



g b (t-T) = V b (t~T) + & b (t-T)Xv b (t-T)-a b (t-T) 



e b (t-T) = m b (t-T)-m b (t-T)- 
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fuerza especffica a b en ejes cuerpo; 

un modulo de navegacion, donde a partir de la fuerza especffica a b y de la 

matriz de cosenos directores B se integran las ecuaciones de navegacion del 
vehfculo para obtener posicion y velocidad calculadas en ejes locales y se corrigen en 
5 un filtro de Kalman, que puede ser estatico, para obtener posicion y velocidad 
estimadas en ejes locales. 

En el caso de una aeronave no tripulada todos estos requerimientos de medios 
para conocer diversos parametros o de adquisicion de datos de sensores pueden 
cumplirse con los sensores de los que habitualmente disponen este tipo de aeronaves, 
10 como se vera en los siguientes apartados, y por tanto, la cantidad de sensores 
necesarios no supone una desventaja de la presente invencion, sino mas bien al 
contrario una ventaja en cuanto se trata de extraer la maxima informacion de los 
sensores de que se dispone. 

Es otro objetivo de la presente invencion proporcionar un metodo de fusion de 
15 sensores para estimar posicion, velocidad y orientacion de un vehfculo, que 
comprende: 

calcular las componentes, segun ejes cuerpo del vehfculo, de dos vectores 
unitarios constantes no colineales g b9 e b a partir de medidas de sensores situados en 

el vehfculo segun los ejes cuerpo de este; 
20 - calcular las componentes, segun ejes tierra, de dichos vectores unitarios 
constantes no colineales g n e t , a partir de medidas de sensores situados en el 
vehfculo que proporcionan posicion en ejes locales; 

medir las tres componentes de velocidad angular Q> b del vehfculo en ejes 

cuerpo; 

25 - corregir la velocidad angular fi) h con una correccion y obtener una 
velocidad angular corregida &> b = S) b + a w ; 

integrar las ecuaciones cinematicas del vehfculo, en funcion de la velocidad 
angular corregida &> h y proporcionar la matriz de transformacion B de ejes tierra a 

ejes cuerpo del vehfculo y la orientacion del vehfculo en forma de angulos de Euler <f> ; 
30 - calcular una estimacion de las componentes en ejes cuerpo de los dos vectores 
unitarios constantes no colineales g b9 e b , donde dicha estimacion se calcula segun: 
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S b = Bg t 
e b = Be t 

obtener la correccion mediante la ley de control: 
u 0 , = cr(g b xg b + e b xe b ) [1] 
siendo a un escalar positivo, 
5 de forma que al aplicar esta correccion a la velocidad angular medida S> b y utilizar 
la velocidad angular resultante d) b -& b +u m como entrada al modulo de integration de 
las ecuaciones cinematicas, estas son estables en sentido ISS y el error en la 

estimation de la matriz de cosenos directores B y de los angulos de Euler <E> se 
mantiene acotado. 

10 

Referencias: 

[refl] Miroslav Krstic, loannis Kanellakopoulos, Petar Kokotovic, "Nonlinear and 

adaptive Control Design", Willey,1995. 
[ref2] E. D. Sontag, Smooth stabilization implies co-prime factorization, IEEE 
15 Transactions on Automatic Control, AC-34 (1989) 

[ref3] E. D. Sontag, Yuang Wang, On Characterizations of the Input-to-State Stability 
Property. 

[ref4] M. Mandea et al., International Geomagnetic Reference Field - Epoch 2000 
Revision Of The IGRF for 2000 - 2005, http://www.nqdc.noaa.aov/IAGA/wa8/ 
20 iarf.html . 26 May 2000. 

[ref5] Brian L.Stevens, Frank L.Lewis, "Aircraft Control and Simulation", Willey 1992 
[ref6] Hans W. Schussler, Peter Stefen, "Some Advanced Topics in Filter Design", in 
Advanced Topics in Signal Processing Prentice Hill 1988. 

Breve description de los dibuios 
25 A continuation se pasa a describir de manera muy breve una serie de dibujos 

que ayudan a comprender mejor la invention y que se relacionan expresamente con 
una realization de dicha invencion que se presenta como un ejemplo no limitativo de 
esta. 

La figura 1 muestra en un diagrama de bloques el sistema de la invencion en 
30 un sistema de navegacion, guiado y control en una aeronave no tripuladas (UAV). 

La figura 2 se muestra el esquema de implementation del sistema de la 
invencion como elemento integrante de un sistema de control de vuelo. 
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La figura 3 presenta un diagrama de bloques general de los elementos de los 
que consta el sistema de la invencion. 

La figura 4 muestra en diagrama de bloques una vista detallada del modulo de 
resolucion de ecuaciones cinematicas. 
5 La figura 5 muestra un diagrama de bloques detallado de los modulos de 

adquisicion y proceso de la velocidad angular y de adquisicion y proceso de datos de 
sensores. 

La figura 6 muestra en diagrama de bloques detallado del modulo de 
navegacion. 

10 Description de una realizacion preferida de la invencion 

Como ejemplo del mejor modo de llevar a la practica la invencion se presenta 
la realizacion de una unidad de fusion de sensores para un sistema de control de vuelo 
de una aeronave no tripulada (UAV) de corto-medio alcance y baja velocidad, sin que 
esto suponga que no sean posibles otras implementaciones. 

1 5 En la figura 1 se presenta el sistema de control de vuelo en una aeronave no 

tripulada 10, constituido por sensores 2, el sistema 1 de la invencion, un sistema de 
control y guiado 3 y actuadores 4. El sistema 1 de la invencion suministra informacion 
de actitud, velocidad y position al sistema de control y guiado 3, que a su vez 
proporciona la correspondiente informacion a los actuadores 4 de la aeronave. Son 

20 entradas del sistema de la invencion la informacion proporcionada por un conjunto de 
sensores, que en esta figura 1 se representan conjuntamente en una unica caja como 
sensores 2. 

En la figura 2 se detallan dichos sensores, que son: 

una Unidad de Medida Inercial (IMU) 21, que incluye un conjunto de tres 
25 giroscopos 211 y tres acelerometros 212 situados segun los ejes cuerpo del vehfculo 
(en este caso se trata de una IMU de estado solido y bajo coste, por ejemplo, 
Crossbow IMU400); 

un magnetometro 22 con capacidad para medir el campo magnetico terrestre 
segun los tres ejes cuerpo del vehfculo; 
30 - un tubo de Pitot 23, que incluye sensores de presion estatica 231 y diferencial 
232; 

dos veletas 24 dotadas de potenciometros para medir los angulos de ataque 
241 y resbalamiento 242; 

un receptor GPS 25 (Global Positioning System) con una unica antena. 
35 La mejor realizacion contemplada del sistema de la invencion es un sistema 
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basado en microprocesador, compuesto por tarjetas PC-104. Como se presenta en la 
figura 2 el sistema consta de una tarjeta de adquisicion analogico-digital 26 (por 
ejemplo, DM6420 de RTD embedded technologies) para adquirir datos del 
magnetometro 22, las veletas 24 y el Pitot 23, una tarjeta de puertos serie RS-232 29 
5 (por ejemplo, CM310 de RTD embedded technologies) para adquirir datos de la IMU 
21 y del GPS 25, una tarjeta de CPU 28 (por ejemplo, CMV6486 de RTD embedded 
technologies) que contiene al microprocesador y la memoria donde residen en forma 
de programa ejecutable los algoritmos de los modulos 11, 12, 13, 14, 15, 16y 17 
(figura 3). El sistema dispone de un bus PC-104 20, que comunica a las diferentes 

10 tarjetas y una tarjeta de alimentacion electrica 27 que tiene como entrada una 
alimentacion estandar de avion de 28 voltios y genera y distribuye a traves de bus los 
+5F ? ±12Fque necesitan las tarjetas. 

En esta realizacion, se toman como vectores unitarios constantes no colineales 
las direcciones de la gravedad local g y de la proyeccion del campo magnetico 

15 terrestre sobre el piano horizontal perpendicular a la gravedad local e , que se 
consideran constantes. Las componentes, tanto en ejes cuerpo como en ejes tierra, de 
estos vectores se calculan, a partir de medidas de sensores mencionados 
anteriormente, en el modulo de adquisicion y proceso de datos de sensores 17. 

Asf, para calcular g b , la fuerza especffica a b (t) medida por los acelerometros 

20 212 se adquiere en el modulo 17 y se retrasa un tiempo r, correspondiente a cinco 
periodos de muestreo, en el FIFO 170 para obtener a b {t-z) y suministrarla al modulo 

178 . De forma similar, la velocidad angular & b {t) medida por los giroscopos 211 se 
adquiere en el modulo de adquisicion y proceso de la velocidad angular 1 1 y se retrasa 
en el FIFO 111 para obtener 3> b (t-T) que se suministra al modulo 178. La presion 

25 estatica p s medida en el sensor 231 y la presion diferencial p d medida en el sensor 

232 se adquieren en el modulo 17 y se procesan en el modulo de calculo de altitud y 
velocidad 171 para obtener v la velocidad verdadera respecto al aire (TAS, true air 
speed) y la altitud h por un procedimiento que es conocido para un experto en la 
materia. Como en este procedimiento se requiere conocer la temperatura ambiente en 
30 el exterior T 0 , esta se suministra inicialmente, antes del vuelo, por ejemplo en un 

fichero de inicializacion. La velocidad verdadera respecto al aire v se suministra al 
modulo 176 junto con el angulo de ataque a , adquirido del sensor 241, y el angulo de 

resbalamiento p , adquirido del sensor 242, y en dicho modulo 176 se calcula el 
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vector velocidad respecto al aire en ejes cuerpo v b {t) de la siguiente forma: 



£> cos or cos j3 

vsin/3 
v sin a cos /? 



Esta velocidad se retrasa un tiempo r para obtener v b (t-r) que se suministra 
al modulo 178. La velocidad v b (t) se deriva numericamente utilizando un filtro de 

5 Savitzky-Golay 179 [ref 6] de orden cuatro con una ventana de 11 muestras, 
calculandose la derivada para la muestra con fndice 5, es decir, la derivada esta 

retrasada t, cinco periodos de muestreo. Esta derivada v b (t - t) se suministra al 

modulo 178 donde, finalmente, se calculan las componentes en ejes cuerpo del vector 

gravedad local g b (t — T) segun: 



que corresponden a las ecuaciones traslacionales del movimiento del vehfculo con las 
hipotesis de "tierra plana" , gravedad constante, acelerometro situado en el centro de 
gravedad del vehfculo y velocidad del viento constante. A la vista de estas ecuaciones 
se justifica la razon de retrasar las diferentes variables. Obtener la derivada numerica 
15 de la velocidad, que es una variable que contiene ruido, no es tarea sencilla; una de 
las mejores soluciones es el filtro de Savitzky-Golay, pero este es un filtro no causal, 
por lo que hay que retrasar el instante en que se calcula la derivada. 

Para calcular e b , en primer lugar se adquiere, en el modulo 17, el valor del 

campo magnetico terrestre rh b (t) medido en el magnetometro 22 y se retrasa un 
20 tiempo t en el FIFO 174 para obtenerse fh b (t-r) y suministrarse al modulo 175. En 
este modulo 175, al que tambien se ha proporcionado g b (t — r) , se realiza el calculo: 



El calculo de las componentes en ejes tierra de los vectores unitarios en la 
direccion de la gravedad local y la proyeccion del campo magnetico terrestre sobre el 
25 piano horizontal perpendicular a la gravedad local g n e t se realiza en el modulo 173. 

En este modulo 173 se implementa un modelo del International Geomagnetic 
Reference Field (IGRF) [ref 4] que proporciona las componentes del campo magnetico 

terrestre en ejes tierra m t dada la posicion del vehfculo P t . De la misma forma un 



10 



g b (t-T) = V b (t-T) + & b (t-T)XV b (t-T)-a b (t-T) 



e b (t -T) = m b (t,-r)- m b (t - t) • 



\g b (t-T)\ 
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10 



15 



20 



modelo de gravedad local proporciona g t a partir de la posicion del vehiculo P t . Para 

obtener esta posicion, en el modulo 17 se adquieren las medidas del GPS 25, se 
procesan en el modulo 172 para convertirlas del sistema de referenda del GPS a ejes 
tierra y la posicion P t asf obtenida se suministra al modulo 173. 

Los tres giroscopos 211 situados segun los ejes cuerpo de la aeronave miden 
las tres componentes de la velocidad angular S> b en estos ejes. Esta es adquirida en 

un modulo de adquisicion y proceso de la velocidad angular 11, donde en un FIFO 111 
se retrasa un tiempo r correspondiente a cinco periodos de muestreo. El modulo 1 1 
suministra velocidad angular actual 5) b (t) al modulo de resolucion de ecuaciones 
cinematicas 13, y velocidad angular retrasada cb b (t-T) al modulo 12, donde le suma 
la correction u^it-T) calculada en el modulo de control 14 para obtener una 
velocidad angular corregida cb b (t-r) = S> b {t-f)-\-u m {t-T) , que tambien se suministra 
al modulo 13. 

En el modulo de resolucion de ecuaciones cinematicas 13 se utiliza el 
quaternion q T = [q 0 q x q 2 q 3 ] como medio de representation de la actitud. 
Inicialmente el quaternion se calcula en el modulo 134 a partir de los angulos de Euler 
= 0 y/\ iniciales que se suministran externamente, segCin: 



^ 0 (/ 0 ) = cos[|]cos[|]cos[^ 



+ sin 



9,(4,) = sin 



2) 12 



Wo 



cos 



K1) 



Wo 
v 2 



— cos 



■sm 



*HfH*) 



u. 



[2.1] 



Las ecuaciones cinematicas del movimiento del vehiculo expresadas en 
funcion del quaternion son: 



#0 




0 


O) 0 




0) 2 


9o 


4i 


_ 1 


-Q) 0 


0 


-eo 2 


(D x 


9x 




~ 2 






0 


-O) 0 


<li 






~°>2 


-co, 


co 0 


0 


.ft 



[2.2] 



que abreviadamente representaremos como: q(t) = ^(ja b (i))q(t) . Integraremos estas 
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ecuaciones en dos fases: 

t-T 

en una primera fase utilizaremos la velocidad angular corregida cb en las ecuaciones 
diferenciales [2.2] y las integraremos en el modulo 137 utilizando, por ejemplo, el 
5 metodo de McKern [ref5] desde el tiempo inicial hasta t-t , ya que solamente se 
dispone de cb b (t~T) y no de cb b (t) en cada instante t. De esta forma obtenemos 
q(t-r) y como se demostro al desarrollar la ley de control sabemos que el error 
q(t - t) = q(t — T)-q(t — T) esta acotado. La segunda fase de la integracion se realiza 
en el modulo 132 donde se integra la derivada del quaternion, calculada en el modulo 
10 131 utilizando la velocidad angular 3) b (t) , desde el instante t-T hasta t, y utilizando 
como valor inicial en t-Te\ quaternion estimado en el modulo 137: q(t-r) . Dado 
que esta segunda fase de la integracion se desarrolla durante un tiempo muy pequeno 
t, correspondiente a cinco muestras, el error cometido por utilizar cd h (t) e vez de 

d) b (t) es despreciable y como resultado se obtiene q(t) que se distribuye al modulo 

15 138 donde se calcula la matriz de transformation de ejes tierra a ejes cuerpo B(f) 
segtin: 

g 2{q x q 2 -q 0 q 3 ) 2(q x q 3 +q 0 q 2 ) 

B= 2{q l q 2 +q { &) 2(^+g£) 
2(q& -q 0 q 2 ) 2(q 2 q 3 % -$ -% 

utilizando el quaternion q(t). De la misma forma, en el modulo 138 se calcula B(t — T) 
utilizando q(t-T) proporcionado por el modulo 137. 

20 Conocidos B(t - t) proporcionado por el modulo 137 y e t (t-r),g t (t-T) 

proporcionados por el 17, en el modulo 15 se sintetiza e b (t-T),g b (t-T) segCin: 

e b (t-T) = B(t-T)e t 
g b (t-r) = B(t-T)g t 

y en el modulo 14 se calcula la correction u^t-r) anterior mediante la ley de control: 
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donde a es una ganancia escalar constante positiva, que se ha seleccionado en este 
caso a un valor de 0.1. 

Como se muestra en la figura 6, el modulo de navegacion 16 consiste en tres 
filtros Kalman estacionarios uno para cada uno de los canales x,y,z, 161, 162, 163 
5 respectivamente, y se calcula en el modulo 164 la aceleracion en ejes tierra a partir de 

la fuerza especffica a b {t) y gravedad g t proporcionadas por el modulo 17 y B(t) 

proporcionada por el modulo 13: 

a t =B T a b +g t 

y se suministra cada componente de a t como senal de control a cada uno de los 
10 filtros. Suponemos que a t contiene ruido exponencialmente correlacionado, pudiendo 
describirse como: 

a t — a t + r 
r = -yr + e r 

donde e r es ruido bianco de media cero y y es diagonal. Por tanto, el vector de 

estado basico consistente en p t V t posicion y velocidad se aumenta con r, 

15 correspondiente al "shaping filter" anterior, para tener en cuenta la correlation en la 
aceleracion. 

Consideramos que la velocidad de la masa de aire respecto a tierra se 
compone de una constante V w , la velocidad del viento, y un ruido correlacionado w g , 
que representa la turbulencia atmosferica y puede representarse por: 







"0 


r 






"0" 






_4> 


4_ 




+ 


1 



20 

donde £ s es ruido bianco con media cero, y las constantes A,,ju se determinan a partir 
de la densidad espectral especificada en [Mil. Spec. 1797]. 

Para poder identificar la velocidad del viento en ejes tierra, incorporaremos el 
"shaping filter" anterior anadiendo tres componentes mas al vector de estado de cada 
25 uno de los filtros: V w9 s Q9 s x . Finalmente el vector de estado queda: 



Si 
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Las ecuaciones de estado son: 

P t =v t 

V t =a t +r 
r = -yr + e r 

s l = A 0 s 0 +A l s 1 +e s 

K = o 

Como medidas, ademas de las del GPS p n V t , incorporamos la velocidad 

aerodinamica en ejes tierra: 

5 v t =B T v b 

que se calcula en el modulo 165. 

Tambien incorporamos la medida de la altura barometrica, pero solo en el filtro 
correspondiente al canal z: 

h-h 0 =- Pn +7j h 
10 Las ecuaciones de medida quedan: 

v t = V t -V w -fi 0 s 0 -^s,+t] v 

P,=Pt+V P 
h-h 0 =-p t2 +7j h 

donde 77 es ruido bianco con media cero. 

Los filtros Kalman quedan: 

i = 0,1,2 

15 donde: 
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i = 0,1,2 
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Pa 
h-h 



10 0 0 
0 10 0 
0 1 0 -ju 0t 
"10 0 0 

0 10 0 

0 1 0 
-10 0 0 
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0 



y las ganancias K, se obtienen resolviendo la correspondiente ecuacion algebraica de 
Riccati dadas las matrices de covarianza del ruido de proceso Q. y medida R { : 

4P { + p t 4 + a - PtfK'cfi = o 

i = 0,1,2 
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REIVIND1CAC1QNES 

1. Sistema para estimar posicion, velocidad y orientacion de un vehfculo (10), que 
comprende: 

medios para determinar las componentes, segun ejes cuerpo del vehfculo, de 
5 dos vectores unitarios constantes no colineales g b ,e b ; 

medios para determinar las componentes, segun ejes tierra, de dichos vectores 
unitarios constantes no colineales g n e t ; 

medios para determinar las tres componentes de velocidad angular 8> b del 

vehfculo en ejes cuerpo; 
10 caracterizado porque el sistema comprende: 

medios para corregir dicha velocidad angular cb b con una correccion y 

obtener una velocidad angular corregida ti> b = S) h + u m ; 

un modulo de integracion de las ecuaciones cinematicas (13) del vehfculo, que 
recibe como entrada la velocidad angular corregida cb b y proporciona la matriz de 

15 transformacion B de ejes tierra a ejes cuerpo del vehfculo y la orientacion del vehfculo 

en forma de angulos de Euler <f> ; 

un modulo de sfntesis (15) de las componentes en ejes cuerpo de los dos 
vectores unitarios constantes no colineales para proporcionar una estimacion de 
dichos vectores no colineales en ejes cuerpo g b ,e b , donde dicha estimacion se calcula 
20 segun: 



un modulo de control (14), que implementa una ley de control para calcular 
dicha correccion u m , donde dicha ley de control es: 



25 siendo a un escalar positivo, 

de forma que al aplicar esta correccion a la velocidad angular medida 3> b y utilizar 

la velocidad angular resultante &> b —&> b +u m como entrada al modulo de integracion de 

las ecuaciones cinematicas, estas son estables en sentido ISS y el error en la 

estimacion de la matriz de cosenos directores B y de los angulos de Euler <f> se 
30 mantiene acotado. 




[1] 



HOJA RECTIFICADA (REGLA 91) ISA/ES 



WO 2005/083358 PCT/ES2005/000088 

-21- 

2. Sistema segun la reivindicacion 1, caracterizado porque dichos vectores 
unitarios no colineales g,e son gravedad local g y proyeccion del campo magnetico 
sobre el piano perpendicular a la gravedad e . 

3. Sistema segun la reivindicacion 2, caracterizado porque los medios para 
5 determinar las componentes, segun ejes cuerpo del vehfculo, de los dos vectores 

unitarios constantes no colineales g b9 e b incluyen: 

una Unidad de Medida Inercial (IMU) (21), que incluye un conjunto de al menos 
tres giroscopos (211) y al menos tres acelerometros (212) situados segun los ejes 
cuerpo del vehfculo; 

10 un magnetometro (22) con capacidad para medir el campo magnetico terrestre 

segtin los ejes cuerpo del vehfculo; 

sensores de presion estatica (231) y diferencial (232); 

dos veletas (24) para medir los angulos de ataque (241) y resbalamiento (242); 
un modulo de adquisicion y proceso (11) de la velocidad angular, configurado 
15 para adquirir la velocidad angular fi> b (t)y retrasarla para obtener S) h (t-T) ; 

un modulo de adquisicion y proceso de datos (17) configurado para adquirir la 
fuerza especffica a b {t) medida por los acelerometros (212), la presion estatica p s (t) 

medida en el sensor (231), la presion diferencial p d (t) medida en el sensor (232), el 

angulo de ataque a(t) medido en el sensor (241), el angulo de resbalamiento fi(t) 
20 medido en el sensor (242) y el valor del campo magnetico terrestre fh b (t) medido en el 
magnetometro (22), retrasarlos y procesarlos para calcular la velocidad verdadera 
respecto al aire v(t-r), la velocidad aerodinamica en ejes cuerpo v b (t-r) segOn: 

v cos a cos P 
v sin a cos ft 



la derivada numerica de la velocidad aerodinamica en ejes cuerpo v b (t — f) , 
25 la gravedad local en ejes cuerpo g b segtin: 

g b (t-T) = V b (t-T) + & b (t-T)Xv b (t-r)-a b (t-T) 
y la proyeccion del campo magnetico terrestre sobre el piano horizontal perpendicular 
a la gravedad local e(t — r) segun: 
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4. Sistema segun la reivindicacion 3, caracterizado porque los medios para 
conocer las componentes, segdn ejes tierra, de los dos vectores unitarios constantes 
nocolineales g n e t incluyen: 

5 - un receptor GPS (25) (Global Positioning System); 

y porque en dicho modulo de adquisicion y proceso de datos (17) se adquieren los 
datos proporcionados por el GPS, se procesan y se utilizan para calcular las 
componentes, segun los ejes tierra, de los dos vectores unitarios constantes no 
colineales g n e t . 

10 5. Sistema segun la reivindicacion 3, caracterizado porque el sistema incluye un 
filtro de Savitzky-Golay (179) en el que se calcula i> 6 , derivada numerica de v b . 

6. Sistema segun la reivindicacion 1 , que incluye: 

medios de adquisicion de datos de un grupo de sensores (25, 231) situados en 
el vehiculo, que proporcionan posicion y velocidad segun ejes tierra P n V t \ 
15 - medios de adquisicion de datos de otro grupo de sensores (212) situados en el 
vehiculo, que proporcionan fuerza especifica a b en ejes cuerpo; 

un modulo de navegacion, donde a partir de la fuerza especifica a b y de la 

matriz de cosenos directores B se integran las ecuaciones de navegacion del 
vehiculo para obtener posicion y velocidad calculadas en ejes locales y se corrigen en 
20 un filtro de Kalman para obtener posicion y velocidad estimadas en ejes locales. 

7. Metodo para estimar posicion, velocidad y orientacion de un vehiculo (10), que 
comprende: 

calcular las componentes, segdn ejes cuerpo del vehiculo, de dos vectores 
unitarios constantes no colineales g b ,e b a partir de medidas de sensores situados en 
25 el vehiculo segun los ejes cuerpo de este; 

calcular las componentes, segun ejes tierra, de dichos vectores unitarios 
constantes no colineales g n e t , a partir de medidas de sensores situados en el 
vehiculo que proporcionan posicion en ejes locales; 

medir las tres componentes de velocidad angular cb b del vehiculo en ejes 

30 cuerpo; 

corregir la velocidad angular S> b con una correccion u m y obtener una 
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velocidad angular corregida cb b = & b +u w ; 

integrar las ecuaciones cinematicas del vehfculo, en funcion de la velocidad 
angular corregida cb b y proporcionar la matriz de transformation B de ejes tierra a 

ejes cuerpo del vehfculo y la orientation del vehfculo en forma de angulos de Euler O ; 
5 - calcular una estimacion de las componentes en ejes cuerpo de los dos vectores 
unitarios constantes no colineales g b ,e b , donde dicha estimacion se calcula segun: 

e b = Be t 

obtener la correccion u m mediante la ley de control: 

Ua>=°(g b *g b + e b xe b ) [1] 

10 siendo a un escalar positivo, 

de forma que al aplicar esta correccion u w a la velocidad angular medida 6> b y utilizar 

la velocidad angular resultante d) b = a> b + Ua) como entrada al modulo de integration de 

las ecuaciones cinematicas, estas son estables en sentido ISS y el error en la 

estimacion de la matriz de cosenos directores B y de los angulos de Euler <f> se 
1 5 mantiene acotado. 

8. Metodo segun la reivindicacion 7, caracterizado porque dichos vectores 
unitarios no colineales g,e son gravedad local g y proyeccion del campo magnetico 
sobre el piano perpendicular a la gravedad e . 

9. Metodo segun la reivindicacion 8, caracterizado porque comprende: 

20 - medir fuerza especffica a b (t) , presion estatica p s (t) , presion diferencial p d (t) , 

angulo de ataque a(t) , angulo de resbalamiento J3(t) y valor del campo magnetico 
terrestre m b (f) \ 

calcular la velocidad verdadera respecto al aire v(i) a partir de las medidas de 

presion diferencial p d (t), presion estatica p s (t) y conocida la temperatura exterior en 

25 el momento inicial T 0 ; 

calcular la velocidad aerodinamica en ejes cuerpo segun: 

v cos a cos p 
v b = vsin p 
v sin a cos ft 
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retrasar un periodo r la velocidad angular 6> b (t) , la fuerza especffica a b (f) , el 
campo magnetico m b (t) y la velocidad aerodinamica en ejes cuerpo v b (t) ; 

calcular la derivada numerica de la velocidad aerodinamica en ejes cuerpo 
v b (t-r)\ 

calcular la gravedad local en ejes cuerpo g b segdn: 

g b (t-T) = %(t-T) + Q b (t-r)xv b (t-T)-a b (t-T);y t 

calcular la proyeccion del campo magnetico terrestre sobre el piano horizontal 
perpendicular a la gravedad local segun: 

e b (t - T) = ih b (t- T) - m b (t - T) ■ ^ " T) , . 

\g b (t-T)\ 



10. Metodo segdn la reivindicacion 9, caracterizado porque se mide la posicion en 
ejes tierra P t y a partir de esta medida se calculan las componentes, segun los ejes 
tierra, de los dos vectores unitarios constantes no colineales g n e t . 

11. Metodo segun cualquiera de las reivindicaciones 9-10, caracterizado porque 
v b , derivada numerica de v b , se calcula en un filtro de Savitzky-Golay (179). 

12. Metodo segun cualquiera de las reivindicaciones 7-11, que incluye: 
medir posicion y velocidad en unos ejes fijos a tierra P n V t ; 

medir fuerza especffica a b en ejes cuerpo; 

integrar las ecuaciones de navegacion del vehfculo en funcion de la fuerza 
especffica a b y de la matriz de cosenos directores B para obtener posicion y 
velocidad calculadas en ejes locales y se corrigen en un filtro de Kalman para 
obtener posicion y velocidad estimadas en ejes locales. 
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